#include "MecanumWheel.h"
#include "motor.h"

void MW_Stop()
{
    Target_FL = 0;
    Target_FR = 0;
    Target_BL = 0;
    Target_BR = 0;
}

void MW_Go_Forward(uint16_t speed)
{
    Target_FL = speed;
    Target_FR = speed;
    Target_BL = speed;
    Target_BR = speed;
}

void MW_Go_Backward(uint16_t speed)
{
    Target_FL = -speed;
    Target_FR = -speed;
    Target_BL = -speed;
    Target_BR = -speed;
}

void MW_Pan_Left(uint16_t speed)
{
    Target_FL = -speed;
    Target_FR =  speed;
    Target_BL =  speed;
    Target_BR = -speed;
}

void MW_Pan_Right(uint16_t speed)
{
    Target_FL =  speed;
    Target_FR = -speed;
    Target_BL = -speed;
    Target_BR =  speed;
}

void MW_Rotate_Clkwise(uint16_t speed)
{
    Target_FL =  speed;
    Target_FR = -speed;
    Target_BL =  speed;
    Target_BR = -speed;
}

void MW_Rotate_Anticlkwise(uint16_t speed)
{
    Target_FL = -speed;
    Target_FR =  speed;
    Target_BL = -speed;
    Target_BR =  speed;
}

void MW_Diagonal_FL(uint16_t speed)
{
    Target_FL =      0;
    Target_FR =  speed;
    Target_BL =  speed;
    Target_BR =      0;
}

void MW_Diagonal_FR(uint16_t speed)
{
    Target_FL =  speed;
    Target_FR =      0;
    Target_BL =      0;
    Target_BR =  speed;
}

void MW_Diagonal_BL(uint16_t speed)
{
    Target_FL = -speed;
    Target_FR =      0;
    Target_BL =      0;
    Target_BR = -speed;
}

void MW_Diagonal_BR(uint16_t speed)
{
    Target_FL =      0;
    Target_FR = -speed;
    Target_BL = -speed;
    Target_BR =      0;
}